#######################################################################
#
# NOTE: How to use mediator.py as a standalone (read: not Breve) script:
# 1) delete the line "import breve"
# 2) delete the class definition "class mediator(breve.Object):"
# 3) substitute it with "class mediator():"
# 4) delete the line "breve.Object.__init__(self)" in the constructor
# 5) delete the "iterate", "archive" and "dearchive" methods
# 6) maybe uncomment some of the stuff at the end of the file
#
#######################################################################


## special import needed by Breve
import breve

## system calls, regular expressions, starting other programs
import os, sys, re, subprocess
## XML parsing
import xml.dom.minidom

## if TRUE, causes mediator.py to inject lots of nice stuff into the SAS file
#  this renders the SAS file pretty much useless, so only use if for debugging!
DEBUG_OUTPUT = False

## Regex for "(1, 2, 3)" --> {'x':1, 'y':2, 'z':3}
RE_VECTOR = re.compile(r'\((?P<x>-?[\d.]*)[,\s]*(?P<y>-?[\d.]*)[,\s]*(?P<z>-?[\d.]*)\)')
## REs for operators
re_move_unladen = re.compile(r'\(move-unladen a(?P<agent>\d+) pos-(?P<x1>\d+)-(?P<y1>\d+) pos-(?P<x2>\d+)-(?P<y2>\d+)\)')
re_move_laden = re.compile(r'\(move-laden a(?P<agent>\d+) pos-(?P<x1>\d+)-(?P<y1>\d+) pos-(?P<x2>\d+)-(?P<y2>\d+)\)')
re_pickup = re.compile(r'\(pick-up-food a(?P<agent>\d+) f(?P<food>\d+) pos-(?P<x>\d+)-(?P<y>\d+)\)')
re_drop = re.compile(r'\(drop-food a(?P<agent>\d+) f(?P<food>\d+) pos-(?P<x>\d+)-(?P<y>\d+)\)')
re_eat = re.compile(r'\(eat-food a(?P<agent>\d+) f(?P<food>\d+)\)')

## the directory for FastDownward's executables
FASTDOWNWARD_EXEC_DIR = "/media/sda2/8semester/lab_course/hg.fast-downward.org/src"
BIN_TRANSLATE  = "/translate/translate.py"
BIN_PREPROCESS = "/preprocess/preprocess"
BIN_SEARCH     = "/search/downward"
INDENT4 = "    "

## default value; overwritten during parsing of agents from simulation state XML 
AGENT_MAX_ENERGY = 30


## Mediator main class
#
#  Mediator is an object that can be imported by Breve. It can read the XML
#  state of a simulation, compile a SAS+ domain/state file from it, run the
#  FastDownward planning system on that file, read back the resulting plan,
#  interpret it, and return it to Breve.
class mediator(breve.Object):
    def __init__(self):
        self.doc = self.agents = self.map = self.foods = self.walls = self.mothership = self.plan = self.p = self.pstep = None
        breve.Object.__init__(self)  ## required by Breve

    ## required by Breve
    def iterate(self):
        pass

    ## Breve method (not required, but removes one error message)
    def archive(self):
        print "INFO: mediator.py: Archiving not implemented."
        return 1

    ## Breve method (not required, but removes one error message)
    def dearchive(self):
        return breve.Abstract.dearchive( self )

    ## parse an XML file
    def parse(self, filename):
        self.doc = xml.dom.minidom.parse(filename)
        self.get_agents()
        self.get_map()
        self.get_foods()
        self.get_walls()
        self.get_mothership()
        print "INFO: mediator.py: '%s' successfully parsed, simulation state loaded." % filename
        

    ## extract the agents' positions
    def get_agents(self):
        global AGENT_MAX_ENERGY
        instances = [n for n in self.doc.getElementsByTagName('instance') if n.getAttribute('class') == 'planAgent']
        ## extract 'grid_position' vectors
        self.agents = []
        for (pddl_index, a) in enumerate(instances):
            vec = [n for n in a.getElementsByTagName('vec') if n.getAttribute('name') == 'grid_position'][0]
            pos_string = vec.firstChild.data
            pos = dict([(k,float(v)) for (k,v) in RE_VECTOR.match(pos_string).groupdict().items()])
            remaining_energy = int(float([n for n in a.getElementsByTagName('flt') if n.getAttribute('name') == 'remaining_energy'][0].firstChild.data))
            AGENT_MAX_ENERGY = int([n for n in a.getElementsByTagName('int') if n.getAttribute('name') == 'max_energy'][0].firstChild.data)
            a = {'pddl_name':'a%d'%pddl_index, 'index': int(a.getAttribute('index')), 'grid_position': pos, 'remaining_energy': remaining_energy}
            self.agents.append(a)


    ## extract the map with entries
    #  self.map is indexed as self.map[y][x]
    def get_map(self):
        ## [0] because we know there should be only one map
        map_element = [n for n in self.doc.getElementsByTagName('instance') if n.getAttribute('class') == 'MarsExplorerWorldMap'][0]
        ## again, there should only be one element 'Map' inside the Map instance
        map_element = [n for n in map_element.getElementsByTagName('class') if n.getAttribute('name') == 'Map'][0]
        ## extract x-axis lists (rows of cells)
        rows = [n for n in self.doc.getElementsByTagName('list') if n.getAttribute('name') == '']
        ## extract entry elements from x-axis lists
        rows = [[int(n.firstChild.data) for n in row.getElementsByTagName('int')] for row in rows]
        ## eliminate 1-element rows (don't know where these are from...)
        rows = [row for row in rows if len(row) > 1]
        self.map = rows


    ## extract the food positions
    def get_foods(self):
        ExtendedFoods_instances = [n for n in self.doc.getElementsByTagName('instance') if n.getAttribute('class') == 'ExtendedFood']
        self.foods = []
        for (pddl_index, f) in enumerate(ExtendedFoods_instances):
            vec = [n for n in f.getElementsByTagName('vec') if n.getAttribute('name') == 'grid_position'][0]
            pos_string = vec.firstChild.data
            pos = dict([(k,float(v)) for (k,v) in RE_VECTOR.match(pos_string).groupdict().items()])
            carriedByAgent = [int(n.getAttribute('index')) for n in f.getElementsByTagName('obj') if n.getAttribute('name') == 'carriedByAgent'][0]
            d = {'pddl_name':'f%d'%pddl_index, 'index': int(f.getAttribute('index')), 'carriedByAgent': carriedByAgent, 'grid_position': pos}
            self.foods.append(d)


    ## extract the wall positions
    def get_walls(self):
        ExtendedWall_instance = [n for n in self.doc.getElementsByTagName('instance') if n.getAttribute('class') == 'ExtendedWall']
        ExtendedWall_instancedata = [n for e in ExtendedWall_instance for n in e.getElementsByTagName('vec') if n.getAttribute('name') == 'grid_position']
        ExtendedWall_grid_position_strings = [n.firstChild.data for n in ExtendedWall_instancedata]
        ExtendedWall_positions = [dict([(k, float(v)) for (k,v) in RE_VECTOR.match(datum).groupdict().items()]) for datum in ExtendedWall_grid_position_strings]
        self.walls = [{'grid_position': w} for w in ExtendedWall_positions]


    ## extract the mothership's position
    #  The mothership has an instance on its own, but no grid_position.
    #  Instead, the mothership's grid_position is an attribute of the controller instance.
    def get_mothership(self):
        ## [0] because there is only one planAgent_Control controller
        Controller = [n for n in self.doc.getElementsByTagName('class') if n.getAttribute('name') == 'planAgent_Control'][0]
        mothership_grid_position_string = [n for n in Controller.getElementsByTagName('vec') if n.getAttribute('name') == 'mothership_grid_position'][0]
        mothership_grid_position = dict([(k, float(v)) for (k,v) in RE_VECTOR.match(mothership_grid_position_string.firstChild.data).groupdict().items()])
        self.mothership = {'grid_position': mothership_grid_position}


    ## helper function to get a "pos-x-y" string from a grid_position dict {'x': x, 'y': y}
    def get_pos(self, d):
        return "pos-%d-%d" % ( d['grid_position']['x'] , d['grid_position']['y'] )


    ## write the problem file for planning (PDDL version)
    #
    #  It is advised NOT to use this function, except for debugging. While fully
    #  functional, the resulting files have to be run through FastDownward's
    #  translator script, which takes several to many minutes, and anyway too long.
    #
    #  Instead, use the "write_sas" method. Unreadable output, but much faster.
    def write_simulation_state_to_pddl(self, problem_name):
        print 'INFO: mediator.py: Compiling PDDL file from simulation state...'
        filename = problem_name + '.pddl'
        data = []
        data.append('(define (problem %s)' % problem_name)
        data.append('')
        data.append('(:domain marsexplorer-planning)')
        
        ## (:objects
        maxx, maxy = len(self.map[0]), len(self.map)
        data.append('(:objects')
        data.append(INDENT4 + '%s - agent' % (' '.join([a['pddl_name'] for a in self.agents])))
        data.append(INDENT4 + 'm0 - mothership')
        data.append(INDENT4 + '%s - food' % (' '.join([f['pddl_name'] for f in self.foods])))
        data.append(INDENT4 + '%s - position' % (' '.join(['pos-%d-%d'%(x,y) for x in range(maxx) for y in range(maxy)])))
        data.append(INDENT4 + ')')
        data.append('')
        # )
        
        ## (:init
        data.append('(:init')
        data.append(INDENT4 + '(not-done)')
        data.append(INDENT4 + '(not-decrease-energy-by-1)')
        data.append(INDENT4 + '(not-decrease-energy-by-2)')
        data.append(INDENT4 + '(not-increase-energy)')
        data.append(INDENT4 + '(= (total-cost) 0)')
        data.append(INDENT4 + '(mothership-at %s)' % self.get_pos(self.mothership))
        for i in range(AGENT_MAX_ENERGY):
            data.append(INDENT4 + '(not-energy-level-%s)' % i)
        data.append(INDENT4 + '(energy-level-%s)' % AGENT_MAX_ENERGY)
        
        for a in self.agents:
            data.append(INDENT4 + '(agent-at %s %s)' % ( a['pddl_name'], self.get_pos(a) ))

        carrying_agents = []
        positions_with_food = []
        for f in self.foods:
            data.append(INDENT4 + '(not-food-processed %s)' % f['pddl_name'])
            ## food on the ground
            if f['carriedByAgent'] == -1:  
                data.append(INDENT4 + '(food-at %s %s)' % ( f['pddl_name'], self.get_pos(f) ))
                data.append(INDENT4 + '(food-here %s)'  % ( self.get_pos(f) ))
                positions_with_food.append(( int(f['grid_position']['x']), int(f['grid_position']['y']) ))
            ## food carried by agent
            else:  
                carriedByAgent = [n for n in self.agents if n['index'] == f['carriedByAgent']][0]
                data.append(INDENT4 + '(food-with %s %s)' % ( f['pddl_name'], carriedByAgent['pddl_name'] ))
                data.append(INDENT4 + '(has-food %s)' % carriedByAgent['pddl_name'])
                carrying_agents.append(carriedByAgent['pddl_name'])
        for a in self.agents:
            if a['pddl_name'] not in carrying_agents:
                data.append(INDENT4 + '(not-has-food %s)' % a['pddl_name'])

        if len(self.map):
            walls = [(int(w['grid_position']['x']), int(w['grid_position']['y'])) for w in self.walls]
            msx, msy = int(self.mothership['grid_position']['x']), int(self.mothership['grid_position']['y'])
            for y in range(len(self.map)):
                for x in range(len(self.map[0])):
                    if (x, y) not in positions_with_food:
                        data.append(INDENT4 + '(not-food-at pos-%d-%d)' % (x, y))
                    if x != msx or y != msy:
                        data.append(INDENT4 + '(not-mothership-at pos-%d-%d)' % (x, y))
                    if (x, y) in walls:
                        data.append(INDENT4 + '(wall-at pos-%d-%d)' % (x, y))
                    else:
                        data.append(INDENT4 + '(not-wall-at pos-%d-%d)' % (x, y))

        ## directions: every map tile writes bidirectional accessibilities for its eastern and southern neighbors
        #  (if these exist and are not walls)
        for y in range(maxy):
            for x in range(maxx):
                if x < maxx-1 and self.map[y][x] != 1 and self.map[y][x+1] != 1:
                    data.append(INDENT4 + '(adjacent-positions pos-%d-%d pos-%d-%d)' % (x, y, x+1, y))
                    data.append(INDENT4 + '(adjacent-positions pos-%d-%d pos-%d-%d)' % (x+1, y, x, y))
                if y < maxy-1 and self.map[y][x] != 1 and self.map[y+1][x] != 1:
                    data.append(INDENT4 + '(adjacent-positions pos-%d-%d pos-%d-%d)' % (x, y, x, y+1))
                    data.append(INDENT4 + '(adjacent-positions pos-%d-%d pos-%d-%d)' % (x, y+1, x, y))
        data.append(INDENT4 + ')')
        data.append('')
        # )

        ## (:goal
        data.append('(:goal')
        data.append(INDENT4 + '(and')
        for f in self.foods:
            data.append(INDENT4 + '(food-processed %s)' % f['pddl_name'] )
        data.append(INDENT4 + '))')
        data.append('')
        # )
        
        data.append('(:metric minimize (total-cost))')
        data.append('')
        data.append(')')

        f = open(filename, 'w')
        f.write('\n'.join(data))
        f.close()
        print 'INFO: mediator.py: %s written.' % filename


    ## write SAS+ output file as input for FastDownward's preprocessor
    def write_sas(self, sas_filename):
        init = []       ## initial state data, must have an entry for EVERY variable
        goal = []       ## goal state
        data = []       ## line buffer for writing the actual output file
        vardict = {}    ## contains variables for referencing by operators

        ## SAS+ stuff
        data.append("begin_version")
        data.append("3")
        data.append("end_version")
        data.append("begin_metric")
        data.append("1")
        data.append("end_metric")

        ## compile variables
        variables = []
        
        ## foods
        for food in self.foods:
            v = []
            for (y, line) in enumerate(self.map):
                for (x, cell) in enumerate(line):
                    if cell != 1:
                        v.append("Atom food-at(%s, pos-%d-%d)" % (food['pddl_name'], x, y))
                        if x == food['grid_position']['x'] and y == food['grid_position']['y']:
                            if DEBUG_OUTPUT: init.append("Food position %s:" % food['pddl_name'])
                            init.append(len(v)-1)
            for agent in self.agents:
                v.append("Atom food-with(%s, %s)" % (food['pddl_name'], agent['pddl_name']))
                if agent['index'] == food['carriedByAgent']:
                    if DEBUG_OUTPUT: init.append("Food carried %s %s:" % (agent['pddl_name'], food['pddl_name']))
                    init.append(len(v)-1)
            v.append("Atom food-consumed(%s)" % food['pddl_name'])
            v.append("<none of those>")
            vardict['food-at %s' % food['pddl_name']] = len(variables)
            variables.append(v)

            ## (the initial state does not have to deal with consumed/processed food)
            v = []
            v.append("Atom food-processed(%s)" % food['pddl_name'])
            v.append("<none of those>")
            vardict['food-processed %s' % food['pddl_name']] = len(variables)
            variables.append(v)
            if DEBUG_OUTPUT: init.append("Food processed %s:" % food['pddl_name'])
            init.append(len(v)-1)
            goal.append(len(variables)-1)

        ## agents
        for agent in self.agents:
            v = []
            for (y, line) in enumerate(self.map):
                for (x, cell) in enumerate(line):
                    if cell != 1:
                        v.append("Atom agent-at(%s, pos-%d-%d)" % (agent['pddl_name'], x, y))
                        if x == agent['grid_position']['x'] and y == agent['grid_position']['y']:
                            if DEBUG_OUTPUT: init.append("Agent position %s:" % agent['pddl_name'])
                            init.append(len(v)-1)
            vardict['agent-at %s' % agent['pddl_name']] = len(variables)
            variables.append(v)

        ## agents holding food?
        for agent in self.agents:
            v = []
            v.append("Atom has-food(%s)" % agent['pddl_name'])
            v.append("<none of those>")
            for food in self.foods:
                if agent['index'] == food['carriedByAgent']:
                    if DEBUG_OUTPUT: init.append("Agent has food %s:" % agent['pddl_name'])
                    init.append(0)
                    break
            else:
                if DEBUG_OUTPUT: init.append("Agent has no food %s:" % agent['pddl_name'])
                init.append(1)
            vardict['has-food %s' % agent['pddl_name']] = len(variables)
            variables.append(v)

        ## energy levels
        for i in range(AGENT_MAX_ENERGY+1):
            v = []
            v.append('Atom energy-level-%d()' % i)
            v.append('<none of those>')
            vardict['energy-level-%d' % i] = len(variables)
            variables.append(v)
            if DEBUG_OUTPUT: init.append('Energy level %d:' % i)
            ## activate exactly the atom describing the agent's current energy level
            if i == self.agents[0]['remaining_energy']:
                init.append(0)
            else:
                init.append(1)

        ## energy changes
        for i in [1,2]:
            v = []
            v.append("Atom decrease-energy-by-%d()" % i)
            v.append("<none of those>")
            vardict['decrease-energy-by-%d' % i] = len(variables)
            variables.append(v)
            if DEBUG_OUTPUT: init.append("Energy decrement %d:" % i)
            init.append(1)  ## energy change atoms are inactive in the initial state
        v = []
        v.append("Atom increase-energy()")
        v.append("<none of those>")
        vardict['increase-energy'] = len(variables)
        variables.append(v)
        if DEBUG_OUTPUT: init.append("Energy increment:")
        init.append(1)

        ## done
        v = []
        v.append("Atom done()")
        v.append("<none of those>")
        vardict['done'] = len(variables)
        variables.append(v)
        if DEBUG_OUTPUT: init.append("Done:")
        init.append(1)  ## if the agent is already in the "done" phase, planning is obsolete

        ## food-here
        for (y, line) in enumerate(self.map):
            for (x, cell) in enumerate(line):
                if cell != 1:      
                    v = []
                    v.append("Atom food-here(pos-%d-%d)" % (x, y))                    
                    v.append("<none of those>")
                    vardict['food-here pos-%d-%d' % (x, y)] = len(variables)
                    variables.append(v)
                    for food in self.foods:
                        if food['grid_position']['x'] == x and food['grid_position']['y'] == y:
                            if DEBUG_OUTPUT: init.append("Food here %s %s:" % (food['pddl_name'], self.get_pos(food)))
                            init.append(0)
                            break
                    else:
                        if DEBUG_OUTPUT: init.append("No food here pos-%d-%d:" % (x, y))
                        init.append(1)


        ## write out variables
        data.append(len(variables))
        for (i, var) in enumerate(variables):
            data.append("begin_variable")
            data.append("var%d" % i)
            data.append("-1")
            data.append("%d" % len(var))
            for atom in var:
                data.append(atom)
            data.append("end_variable")

        ## no mutex groups
        #  (actually, many mutex groups, but those are all implicit in the variables' fact names)
        data.append("0")

        ## write out initial state
        data.append("begin_state")
        for i in init:
            data.append(i)
        data.append("end_state")


        ## write out goal state
        data.append("begin_goal")
        data.append(len(goal))
        for i in goal:
            data.append("%d 0" % i)
        data.append("end_goal")


        ## compile operators
        operators = []

        ## function-in-function: compiling move operators
        def moveop(agent, x1, y1, x2, y2, laden):
            o = {}
            prefix = ''
            if not laden: prefix = 'un'
            o['name'] = 'move-%sladen %s pos-%d-%d pos-%d-%d' % (prefix, agent['pddl_name'], x1, y1, x2, y2)
            
            preconds = []
            v = vardict['done']
            preconds.append((v, 1))
            v = vardict['increase-energy']
            preconds.append((v, 1))
            v = vardict['energy-level-0']
            preconds.append((v, 1))
            if laden:
                v = vardict['decrease-energy-by-1']
                preconds.append((v, 1))
                v = vardict['has-food %s' % agent['pddl_name']]
                preconds.append((v, 0))
                v = vardict['energy-level-1']
                preconds.append((v, 1))
            else:
                v = vardict['decrease-energy-by-2']
                preconds.append((v, 1))
                v = vardict['has-food %s' % agent['pddl_name']]
                preconds.append((v, 1))
            o['preconds'] = preconds
            
            effects = []
            if laden:
                e = vardict['decrease-energy-by-2']
                effects.append((0, e, 1, 0))
            else:
                e = vardict['decrease-energy-by-1']
                effects.append((0, e, 1, 0))
            e = vardict['agent-at %s' % agent['pddl_name']]
            effects.append((0, e, variables[e].index('Atom agent-at(%s, pos-%d-%d)' % (agent['pddl_name'], x1, y1)), \
                                  variables[e].index('Atom agent-at(%s, pos-%d-%d)' % (agent['pddl_name'], x2, y2))))
            o['effects'] = effects
            o['cost'] = 1
            return o


        ## move
        for (y, line) in enumerate(self.map):
            for (x, cell) in enumerate(line):
                if self.map[y][x] == 1:
                    continue
                if x < len(self.map[0])-1 and self.map[y][x+1] != 1:
                    for agent in self.agents:
                        operators.append(moveop(agent, x, y, x+1, y, False))
                        operators.append(moveop(agent, x, y, x+1, y, True))
                        operators.append(moveop(agent, x+1, y, x, y, False))
                        operators.append(moveop(agent, x+1, y, x, y, True))
                if y < len(self.map)-1 and self.map[y+1][x] != 1:
                    for agent in self.agents:
                        operators.append(moveop(agent, x, y, x, y+1, False))
                        operators.append(moveop(agent, x, y, x, y+1, True))
                        operators.append(moveop(agent, x, y+1, x, y, False))
                        operators.append(moveop(agent, x, y+1, x, y, True))


        ## function-in-function: compiling pick-up operators
        def pickupop(agent, food, x, y):
            o = {}
            o['name'] = 'pick-up-food %s %s pos-%d-%d' % (agent['pddl_name'], food['pddl_name'], x, y)
            
            preconds = []
            v = vardict['done']
            preconds.append((v, 1))
            v = vardict['increase-energy']
            preconds.append((v, 1))
            v = vardict['decrease-energy-by-1']
            preconds.append((v, 1))
            v = vardict['decrease-energy-by-2']
            preconds.append((v, 1))
            v = vardict['agent-at %s' % agent['pddl_name']]
            preconds.append((v, variables[v].index('Atom agent-at(%s, pos-%d-%d)' % (agent['pddl_name'], x, y))))
            o['preconds'] = preconds
            
            effects = []
            e = vardict['has-food %s' % agent['pddl_name']]
            effects.append((0, e, 1, 0))
            e = vardict['food-here pos-%d-%d' % (x, y)]
            effects.append((0, e, 0, 1))
            e = vardict['food-at %s' % food['pddl_name']]
            effects.append((0, e, variables[e].index('Atom food-at(%s, pos-%d-%d)' % (food['pddl_name'], x, y)), \
                                  variables[e].index('Atom food-with(%s, %s)' % (food['pddl_name'], agent['pddl_name']))))
            o['effects'] = effects
            o['cost'] = 1
            return o
            

        ## pickup
        for (y, line) in enumerate(self.map):
            for (x, cell) in enumerate(line):
                ## can't pick up food that's been delivered to the mothership
                if self.map[y][x] == 1 or (self.mothership['grid_position']['x'] == x and self.mothership['grid_position']['y'] == y):
                    continue
                else:
                    for agent in self.agents:
                        for food in self.foods:
                            operators.append(pickupop(agent, food, x, y))


        ## function-in-function: compiling drop operators
        #  'version' distinguishes between the two operators resulting from the original's disjunctive precondition
        def dropop(agent, food, x, y, version):
            o = {}
            o['name'] = 'drop-food %s %s pos-%d-%d' % (agent['pddl_name'], food['pddl_name'], x, y)
            
            preconds = []
            v = vardict['done']
            preconds.append((v, 1))
            v = vardict['increase-energy']
            preconds.append((v, 1))
            v = vardict['decrease-energy-by-1']
            preconds.append((v, 1))
            v = vardict['decrease-energy-by-2']
            preconds.append((v, 1))
            v = vardict['agent-at %s' % agent['pddl_name']]
            preconds.append((v, variables[v].index('Atom agent-at(%s, pos-%d-%d)' % (agent['pddl_name'], x, y))))
            o['preconds'] = preconds
            
            effects = []
            e= vardict['food-here pos-%d-%d' % (x, y)]
            num = 1
            if version == 'mothership': num = -1
            effects.append((0, e, -1, 0))
            e = vardict['has-food %s' % agent['pddl_name']]
            effects.append((0, e, 0, 1))
            e = vardict['food-at %s' % food['pddl_name']]
            effects.append((0, e, variables[e].index('Atom food-with(%s, %s)' % (food['pddl_name'], agent['pddl_name'])), \
                                  variables[e].index('Atom food-at(%s, pos-%d-%d)' % (food['pddl_name'], x, y))))
            o['effects'] = effects
            o['cost'] = 1
            return o

        ## drop
        for (y, line) in enumerate(self.map):
            for (x, cell) in enumerate(line):
                ## can't drop on a wall
                if self.map[y][x] == 1:
                    continue
                else:
                    for agent in self.agents:
                        for food in self.foods:
                            ## there is at most one drop operator for every cell, so the disjunctive
                            #  precondition does not lead to splitting into two operators IN THIS CASE
                            if self.mothership['grid_position']['x'] == x and self.mothership['grid_position']['y'] == y:
                                operators.append(dropop(agent, food, x, y, 'mothership'))
                            else:
                                operators.append(dropop(agent, food, x, y, 'other'))


        ## function-in-function: compiling food consumption operators
        def eatop(agent, food):
            o = {}
            o['name'] = 'eat-food %s %s' % (agent['pddl_name'], food['pddl_name'])
            
            preconds = []
            v = vardict['done']
            preconds.append((v, 1))
            v = vardict['decrease-energy-by-1']
            preconds.append((v, 1))
            v = vardict['decrease-energy-by-2']
            preconds.append((v, 1))
            o['preconds'] = preconds
            
            effects = []
            e = vardict['increase-energy']
            effects.append((0, e, 1, 0))
            e = vardict['has-food %s' % agent['pddl_name']]
            effects.append((0, e, 0, 1))
            e = vardict['food-at %s' % food['pddl_name']]
            effects.append((0, e, variables[e].index('Atom food-with(%s, %s)' % (food['pddl_name'], agent['pddl_name'])), \
                                  variables[e].index('Atom food-consumed(%s)' % food['pddl_name'])))
            o['effects'] = effects
            o['cost'] = 1
            return o

        ## eat
        for agent in self.agents:
            for food in self.foods:
                operators.append(eatop(agent, food))


        ## stop-operations
        o = {}
        o['name'] = 'stop-operations'
        
        preconds = []
        v = vardict['increase-energy']
        preconds.append((v, 1))
        v = vardict['decrease-energy-by-1']
        preconds.append((v, 1))
        v = vardict['decrease-energy-by-2']
        preconds.append((v, 1))
        v = vardict['has-food %s' % agent['pddl_name']]
        preconds.append((v, 1))
        o['preconds'] = preconds
        
        effects = []
        e = vardict['done']
        effects.append((0, e, 1, 0))
        
        o['effects'] = effects
        o['cost'] = 1
        operators.append(o)


        ## function-in-function: compiling soft-goal fulfilling operators
        def tidyupatmothership(food):
            o = {}
            o['name'] = 'tidyup-at-mothership %s %s' % (food['pddl_name'], self.get_pos(self.mothership))
            
            preconds = []
            v = vardict['done']
            preconds.append((v, 0))
            o['preconds'] = preconds
            
            effects = []
            e = vardict['food-processed %s' % food['pddl_name']]
            effects.append((0, e, 1, 0))
            e = vardict['food-at %s' % food['pddl_name']]
            effects.append((0, e, variables[e].index('Atom food-at(%s, %s)' % (food['pddl_name'], self.get_pos(self.mothership))), \
                                  len(variables[e])-1))
            o['effects'] = effects
            o['cost'] = 1
            return o

        ## function-in-function: compiling soft-goal fulfilling operators
        def tidyupconsumed(food):
            o = {}
            o['name'] = 'tidyup-consumed %s' % (food['pddl_name'])
            
            preconds = []
            v = vardict['done']
            preconds.append((v, 0))
            o['preconds'] = preconds
            
            effects = []
            e = vardict['food-processed %s' % food['pddl_name']]
            effects.append((0, e, 1, 0))
            e = vardict['food-at %s' % food['pddl_name']]
            effects.append((0, e, variables[e].index('Atom food-consumed(%s)' % (food['pddl_name'])), \
                                  len(variables[e])-1))
            o['effects'] = effects
            o['cost'] = 30
            return o

        
        ## function-in-function: compiling soft-goal fulfilling operators
        def tidyupinfield(food, x, y):
            o = {}
            o['name'] = 'tidyup-in-field %s pos-%d-%d' % (food['pddl_name'], x, y)
            
            preconds = []
            v = vardict['done']
            preconds.append((v, 0))
            o['preconds'] = preconds
            
            effects = []
            e = vardict['food-processed %s' % food['pddl_name']]
            effects.append((0, e, 1, 0))
            e = vardict['food-at %s' % food['pddl_name']]
            effects.append((0, e, variables[e].index('Atom food-at(%s, pos-%d-%d)' % (food['pddl_name'], x, y)), \
                                  len(variables[e])-1))
            o['effects'] = effects
            o['cost'] = 30
            return o

        ## tidyup
        for food in self.foods:
            operators.append(tidyupatmothership(food))
            operators.append(tidyupconsumed(food))
            for (y, line) in enumerate(self.map):
                for (x, cell) in enumerate(line):
                    if self.map[y][x] == 1:
                        continue
                    else:
                        operators.append(tidyupinfield(food, x, y))


        ## function-in-function: compiling energy-changing operators
        def decreaseenergy(state, by):
            o = {}
            o['name'] = 'decrease-energy-by-%d-from-%d' % (by, state)
            o['preconds'] = []
            
            effects = []
            e = vardict['decrease-energy-by-%d' % by]
            effects.append((0, e, 0, 1))
            e = vardict['energy-level-%d' % state]
            effects.append((0, e, 0, 1))
            e = vardict['energy-level-%d' % (state - by)]
            effects.append((0, e, -1, 0))
            o['effects'] = effects
            o['cost'] = 1
            return o

        ## energy in/decrement
        for i in range(1, AGENT_MAX_ENERGY+1):
            operators.append(decreaseenergy(i, 1))
        for i in range(2, AGENT_MAX_ENERGY+1):
            operators.append(decreaseenergy(i, 2))
        o = {}
        o['name'] = 'increase-energy'
        o['preconds'] = []
        effects = []
        e = vardict['increase-energy']
        effects.append((0, e, 0, 1))
        e = vardict['energy-level-%d' % AGENT_MAX_ENERGY]
        effects.append((0, e, -1, 0))
        for i in range(AGENT_MAX_ENERGY):
            e = vardict['energy-level-%d' % i]
            effects.append((0, e, -1, 1))
        o['effects'] = effects
        o['cost'] = 1
        operators.append(o)
                        

        ## write out operators
        data.append(len(operators))
        for (i, op) in enumerate(operators):
            data.append("begin_operator")
            data.append(op['name'])
            data.append(len(op['preconds']))
            for p in op['preconds']:
                data.append("%d %d" % (p[0], p[1]))
            data.append(len(op['effects']))
            for e in op['effects']:
                data.append("%d %d %d %d" % (e[0], e[1], e[2], e[3]))
            data.append(op['cost'])
            data.append("end_operator")
        

        ## no axioms
        data.append("0")  

        ## write the actual file to disk
        f = open("%s.sas" % sas_filename, 'w')
        f.writelines(["%s\n"%line for line in data])
        f.close()
        print 'INFO: mediator.py: %s.sas written.' % sas_filename
                            


    ## write the domain file for planning
    #  (don't use this; see "write_simulation_state_to_pddl")
    def write_domain_pddl(self, domain_filename):
        print 'INFO: mediator.py: Compiling PDDL file for problem domain...'
        filename = domain_filename + '.pddl'
        data = []
        data.append("""(define (domain marsexplorer-planning)

(:requirements :typing :action-costs)
(:types agent food wall mothership position)

(:predicates
    (agent-at ?a - agent ?p - position)
    (food-at ?f - food ?p - position)
    (food-with ?f - food ?a - agent)
    (has-food ?a - agent)
    (not-has-food ?a - agent)
    (mothership-at ?p - position)
    (not-mothership-at ?p - position)
    (wall-at ?p - position)
    (not-wall-at ?p - position)
    (food-here ?p - position)
    (not-food-here ?p - position)
    (adjacent-positions ?p1 ?p2 - position)
    (food-processed ?f - food)
    (not-food-processed ?f - food)
    (food-consumed ?f - food)
    (done)
    (not-done)
    (decrease-energy-by-1)
    (not-decrease-energy-by-1)
    (decrease-energy-by-2)
    (not-decrease-energy-by-2)
    (increase-energy)
    (not-increase-energy)""")
    
        for i in range(AGENT_MAX_ENERGY+1):
            data.append('    (energy-level-%d)' % i)
            data.append('    (not-energy-level-%d)' % i)
            
        data.append("""    )

(:functions
    (total-cost) - number
    )
    
;;; An agent moves while not holding anything (lower cost).
(:action move-unladen
    :parameters     (?a - agent ?from ?to - position)
    :precondition   (and (not-done)
                         (agent-at ?a ?from)
                         (not-has-food ?a)
                         (not-wall-at ?to)
                         (adjacent-positions ?from ?to)
                         (not-energy-level-0)
                         (not-decrease-energy-by-1)
                         (not-decrease-energy-by-2)
                         (not-increase-energy))
    :effect         (and (increase (total-cost) 1)
                         (decrease-energy-by-1)
                         (not (not-decrease-energy-by-1))
                         (not (agent-at ?a ?from))
                         (agent-at ?a ?to))
    )

;;; An agent moves while holding food (higher cost).
(:action move-laden
    :parameters     (?a - agent ?from ?to - position)
    :precondition   (and (not-done)
                         (agent-at ?a ?from)
                         (has-food ?a)
                         (not-wall-at ?to)
                         (adjacent-positions ?from ?to)
                         (not-energy-level-0)
                         (not-energy-level-1)
                         (not-decrease-energy-by-1)
                         (not-decrease-energy-by-2)
                         (not-increase-energy))
    :effect         (and (increase (total-cost) 1)
                         (decrease-energy-by-2)
                         (not (not-decrease-energy-by-2))
                         (not (agent-at ?a ?from))
                         (agent-at ?a ?to))
    )

;;; An agent can pick up food if it's not already holding one.
;;; A food that is already at the mothership cannot picked up again.
(:action pick-up-food
    :parameters     (?a - agent ?f - food ?where - position)
    :precondition   (and (not-done)
                         (agent-at ?a ?where)
                         (food-at ?f ?where)
                         (food-here ?where)
                         (not-mothership-at ?where)
                         (not-has-food ?a)
                         (not-decrease-energy-by-1)
                         (not-decrease-energy-by-2)
                         (not-increase-energy))
    :effect         (and (increase (total-cost) 1)
                         (has-food ?a)
                         (not (not-has-food ?a))
                         (food-with ?f ?a)
                         (not (food-at ?f ?where))
                         (not (food-here ?where))
                         (not-food-here ?where))
    )

;;; An agent can drop food it's holding, if there is no other food at the position.
;;; Exception: At the mothership, many foods can be dropped
(:action drop-food
    :parameters     (?a - agent ?f - food ?where - position)
    :precondition   (and (not-done)
                         (agent-at ?a ?where)
                         (food-with ?f ?a)
                         (has-food ?a)
                         (or (mothership-at ?where)
                             (not-food-here ?where))
                         (not-decrease-energy-by-1)
                         (not-decrease-energy-by-2)
                         (not-increase-energy))
    :effect         (and (increase (total-cost) 1)
                         (not (food-with ?f ?a))
                         (not (has-food ?a))
                         (not-has-food ?a)
                         (food-at ?f ?where)
                         (food-here ?where)
                         (not (not-food-here ?where)))
    )

;;; An agent can eat food it's holding to increase its energy level.
;;; IMPORTANT: mark the food as consumed, else the agent can never fulfil the goal after consuming any food!
(:action eat-food
    :parameters     (?a - agent ?f - food)
    :precondition   (and (not-done)
                         (has-food ?a)
                         (food-with ?f ?a)
                         (not-decrease-energy-by-1)
                         (not-decrease-energy-by-2)
                         (not-increase-energy))
    :effect         (and (increase (total-cost) 1)
                         (not (has-food ?a))
                         (not-has-food ?a)
                         (not (food-with ?f ?a))
                         (increase-energy)
                         (not (not-increase-energy))
                         (food-consumed ?f))
    )

;;; Action to signal the end of agent operations and commencement of tidy-up operations.
(:action stop-operations
    :precondition   (and (not-done)
                         (not-decrease-energy-by-1)
                         (not-decrease-energy-by-2)
                         (not-increase-energy)
                         (not-has-food a0))
    :effect         (and (increase (total-cost) 1)
                         (done)
                         (not (not-done)))
    )

;;; Action at end: Reward for collected food.
(:action tidyup-at-mothership
    :parameters      (?f - food ?where - position)
    :precondition   (and (done)
                         (food-at ?f ?where)
                         (food-here ?where)
                         (mothership-at ?where)
                         (not-food-processed ?f))
    :effect         (and (increase (total-cost) 1)
                         (not (food-at ?f ?where))
                         (not (food-here ?where))
                         (not-food-here ?where)
                         (food-processed ?f)
                         (not (not-food-processed ?f)))
    )

;;; Action at end: Punishment for uncollected/consumed food.
(:action tidyup-in-field
    :parameters      (?f - food ?where - position)
    :precondition   (or (and (done)
                             (food-consumed ?f))
                        (and (done)
                             (food-at ?f ?where)
                             (food-here ?where)
                             (not-food-processed ?f)))
    :effect         (and (increase (total-cost) 1)
                         (not (food-at ?f ?where))
                         (not (food-here ?where))
                         (not-food-here ?where)
                         (food-processed ?f)
                         (not (not-food-processed ?f))
                         (increase (total-cost) 30))
    )
""")

        data.append(""";;; Actions to increase/decrease the energy level""")
        
        for i in range(1, AGENT_MAX_ENERGY+1):
            data.append("""
(:action decrease-energy-by-1-from-%d
    :precondition (and (decrease-energy-by-1)
                       (energy-level-%d))
    :effect       (and (increase (total-cost) 1)
                       (not (decrease-energy-by-1))
                       (not-decrease-energy-by-1)
                       (not (energy-level-%d))
                       (not-energy-level-%d)
                       (energy-level-%d)
                       (not (not-energy-level-%d)))
    )""" % (i, i, i, i, i-1, i-1))
    
        for i in range(2, AGENT_MAX_ENERGY+1):
            data.append("""
(:action decrease-energy-by-2-from-%d
    :precondition (and (decrease-energy-by-2)
                       (energy-level-%d))
    :effect       (and (increase (total-cost) 1)
                       (not (decrease-energy-by-2))
                       (not-decrease-energy-by-2)
                       (not (energy-level-%d))
                       (not-energy-level-%d)
                       (energy-level-%d)
                       (not (not-energy-level-%d)))
    )""" % (i, i, i, i, i-2, i-2))

        data.append("""
(:action increase-energy
    :precondition (increase-energy)
    :effect (and (increase (total-cost) 1)""")
        data.append("""                 (not (increase-energy))""")
        data.append("""                 (not-increase-energy)""")
        for i in range(AGENT_MAX_ENERGY):
            data.append("""                 (not (energy-level-%d))""" % i)
            data.append("""                 (not-energy-level-%d)""" % i)
        data.append("""                 (not (not-energy-level-%d))""" % AGENT_MAX_ENERGY)
        data.append("""                 (energy-level-%d))
    )""" % AGENT_MAX_ENERGY)

        data.append(""")""")
        
        f = open(filename, 'w')
        f.write('\n'.join(data))
        f.close()
        print 'INFO: mediator.py: %s written.' % filename


    ## tell breve whether the planning has finished
    def check_planning_status(self):
        ## not done yet
        if self.p is None or self.p.poll() is None:
            return -1
            
        if self.pstep == 'None':
            print "ERROR: mediator.py: Planning has not been initialized!"
            return -1
        #~ elif self.pstep == 'translate':
            #~ self.pstep = 'preprocess'
            #~ print "INFO: mediator.py: Planner is preprocessing..."
            #~ f = open('output.sas', 'r')
            #~ self.p = subprocess.Popen([FASTDOWNWARD_EXEC_DIR+BIN_PREPROCESS], stdin=f)
            #~ f.close()            
            #~ return -1
        elif self.pstep == 'preprocess':
            self.pstep = 'search'
            print "INFO: mediator.py: Planner is searching..."
            f = open('output', 'r')
            self.p = subprocess.Popen([FASTDOWNWARD_EXEC_DIR+BIN_SEARCH, '--heuristic', 'hmars=marsexplorer()', '--search', 'lazy_greedy(hmars, preferred=hmars)'], stdin=f)
            f.close()
            return -1
        elif self.pstep == 'search':
            self.pstep = None
            return 0

    ## call the planner toolchain and (eventually) retrieve the plan
    def start_planning(self, sas_filename):
        if self.pstep is not None:
            return
            
        self.pstep = 'preprocess'
        print "INFO: mediator.py: Planner is preprocessing..."
        f = open('%s.sas' % sas_filename, 'r')
        self.p = subprocess.Popen([FASTDOWNWARD_EXEC_DIR+BIN_PREPROCESS], stdin=f)
        f.close()
        
        
    #~ def start_planning(self, problem_name, domain_filename):
        #~ if self.pstep is not None:
            #~ return
            #~ 
        #~ this_dir = os.getcwd()
        #~ domain_file  = this_dir + '/' + domain_filename + '.pddl'
        #~ problem_file = this_dir + '/' + problem_name + '.pddl'

        #~ #TODO (nikolaus): i had to edit FastDownward's translate.py for this to work (extend sys.path)
        #~ print "INFO: mediator.py: Planner is translating..."
        #~ self.p = subprocess.Popen([FASTDOWNWARD_EXEC_DIR+BIN_TRANSLATE, domain_file, problem_file])
        #~ self.pstep = 'translate'
        
        #~ print "INFO: mediator.py: Planner is preprocessing..."
        #~ f = open('output.sas', 'r')
        #~ subprocess.call([FASTDOWNWARD_EXEC_DIR+BIN_PREPROCESS], stdin=f)
        #~ f.close()
        #~ 
        #~ print "INFO: mediator.py: Planner is searching..."
        #~ f = open('output', 'r')
        #~ subprocess.call([FASTDOWNWARD_EXEC_DIR+BIN_SEARCH, '--heuristic', 'hmars=marsexplorer()', '--search', 'lazy_greedy(hmars, preferred=hmars)'], stdin=f)
        #~ f.close()


    ## read an "sas_plan" file and convert the plan into a form usable by the breve caller (i.e., numbers)
    def read_plan(self, plan_filename='sas_plan'):
        try:
            f = open(plan_filename, 'r')
            d = [l.strip() for l in f.readlines()]
            f.close()
        except:
            print "ERROR: mediator.py: Could not read plan file '%s'" % plan_filename
            return
        
        REs = [re_move_unladen, re_move_laden, re_pickup, re_drop, re_eat]

        ## iterate over the plan steps
        plan = []
        for line in d:
            for re in REs:
                match = re.match(line)
                if match is not None:
                    plan.append(plan_translation(re, match, line))
                    break
        self.plan = plan
        print "INFO: mediator.py: Plan file %s read and converted." % plan_filename
        return plan
        

## generates a breve-usable number from a SAS plan step
def plan_translation(re, match, line):
    if re == re_move_unladen or re == re_move_laden:
        x1, y1, x2, y2 = int(match.groupdict()['x1']), int(match.groupdict()['y1']), int(match.groupdict()['x2']), int(match.groupdict()['y2'])
        if y2 == y1 - 1:
            return 0
        elif x2 == x1 + 1:
            return 1
        elif y2 == y1 + 1:
            return 2
        elif x2 == x1 - 1:
            return 3
        else:
            print "ERROR: mediator.py: Could not parse plan step %s" % line
    elif re == re_pickup:
        return 4
    elif re == re_eat:
        return 5
    elif re == re_drop:
        return 6
    else:
        print "ERROR: mediator.py: Could not parse plan step %s" % line


## the rest of this file can be useful if mediator.py is run as a standalone program
#  Breve, on the other hand, WILL CRASH if anything is uncommented here

#~ m = mediator()
#~ m.parse('current_state.xml')
#print "Agents:"
#~ for a in m.agents: print a
#print "Foods:"
#for f in m.foods:  print f
#print "Walls:", "\t".join(['(%d, %d)'%(e['grid_position']['x'],e['grid_position']['y']) for e in m.walls])
#print "Mothership:", m.mothership
#~ m.write_domain_pddl('domain')
#~ m.write_simulation_state_to_pddl('test')
#~ m.write_sas('output')
#~ m.do_plan('test', 'domain')
#~ m.read_plan()
